package novemberBot.navigation;
import lejos.nxt.Button;
import lejos.nxt.Motor;
/**Modified version of the 2 wheeled robot class provided by Patrick Diez in for the labs
 * @author Antoine Lennartz
 * @author Patrick Diez*/
public class TwoWheeledRobot {
	public static final double  DEFAULT_RIGHT_RADIUS = 2.6317, DEFAULT_LEFT_RADIUS =2.6323;
	public static final double DEFAULT_WIDTH = 13.23, SENSOR_DIST = 3.45; //3.918
	protected Motor leftMotor;
	protected Motor rightMotor;
	private double leftWheelRadius, rightWheelRadius, width;
	private double forwardSpeed, rotationSpeed;
	
	public TwoWheeledRobot(Motor leftMotor,
						   Motor rightMotor,
						   double width,
						   double leftWheelRadius,
						   double rightWheelRadius){
		this.leftMotor = leftMotor;
		this.rightMotor = rightMotor;
		this.rightWheelRadius = rightWheelRadius;
		this.leftWheelRadius = leftWheelRadius;
		this.width = width;
	}
	
	public TwoWheeledRobot(Motor leftMotor, Motor rightMotor) {
		this(leftMotor, rightMotor, DEFAULT_WIDTH, DEFAULT_LEFT_RADIUS, DEFAULT_RIGHT_RADIUS);
	}
	
	public TwoWheeledRobot(Motor leftMotor, Motor rightMotor, double width) {
		this(leftMotor, rightMotor, width, DEFAULT_LEFT_RADIUS, DEFAULT_RIGHT_RADIUS);
	}
	
	// accessors
	public int getLeftTacho(){
		return leftMotor.getTachoCount();
	}
	
	public int getRightTacho(){
		return rightMotor.getTachoCount()	;
	}
	public double getLeftWheelRadius(){
		return leftWheelRadius;
	}
	public double getRightWheelRadius(){
		return rightWheelRadius;
	}
	public double getWidth(){
		return width;
	}
	
	public void resetTachos(){
		rightMotor.resetTachoCount();
		leftMotor.resetTachoCount();
	}
	
	// mutators
	public void setForwardSpeed(double speed) {
		forwardSpeed = speed;
		setSpeeds(forwardSpeed, rotationSpeed);
	}
	
	
	public void setRotationSpeed(double speed) {
		rotationSpeed = speed;
		setSpeeds(forwardSpeed, rotationSpeed);
	}
	/**sets wheel speeds based on forward a rotation speeds specified in cm/s and degrees/s*
	 * @param forwardSpeed - speed to move forward in cm/s
	 * @param rotationSpeed -  speed to turn by degrees/s positive = clockwise*/
	public void setSpeeds(double forwardSpeed, double rotationSpeed) {
		double leftSpeed, rightSpeed;

		this.forwardSpeed = forwardSpeed;
		this.rotationSpeed = rotationSpeed; 
		
		leftSpeed=forwardSpeed*180.0/(leftWheelRadius*Math.PI)+ rotationSpeed*width/(2*leftWheelRadius);
		rightSpeed=forwardSpeed*180.0/(rightWheelRadius*Math.PI)- rotationSpeed*width/(2*rightWheelRadius);

		// set motor directions
		if (leftSpeed > 0.0 && leftMotor.getMode()!=1)
			leftMotor.forward();
		else if(leftMotor.getMode()!=2 && leftSpeed<0.0){
			leftMotor.backward();
			leftSpeed = -leftSpeed;
		}
		
		if (rightSpeed > 0.0 && rightMotor.getMode()!=1)
			rightMotor.forward();
		else if (rightSpeed < 0.0 && rightMotor.getMode()!=2){
			rightMotor.backward();
			rightSpeed = -rightSpeed;
		}
		// set motor speeds
		if (leftSpeed > 900.0 && leftMotor.getSpeed()!=900)
			leftMotor.setSpeed(900);
		else if(leftMotor.getSpeed()!=(int)leftSpeed && leftSpeed<=900){
			//while(Math.abs(leftSpeed-leftMotor.getSpeed())>5) leftSpeed*=0.8;
			leftMotor.setSpeed((int)leftSpeed);
		}
		
		if (rightSpeed > 900.0 && rightMotor.getSpeed()!=900)
			rightMotor.setSpeed(900);
		else if(rightMotor.getSpeed()!=(int)rightSpeed && rightSpeed<=900)
			//while(Math.abs(rightSpeed-rightMotor.getSpeed())>5) rightSpeed*=0.8;
			rightMotor.setSpeed((int)rightSpeed);
	}
	/**DEPRECATED
	 * @return if going right||straight return 1 if going left return -1 if straight return 0*/
	public int lean(){
		if(leftMotor.getSpeed()>=rightMotor.getSpeed()) return 1;
		else return -1;
	}
	/**calls Motor.flt() on both wheels
	 * @see lejos.nxt.Motor#flt()
	 * */
	public void floatWheels(){
		rightMotor.flt();
		leftMotor.flt();
	}
	
	public void stop() {
		setSpeeds(0,0);
		rightMotor.stop();
		leftMotor.stop();
	}
	
	/**rotates the robot about it's left wheel
	 * @param speed - speed in tacho/s for right wheel
	 * @param direction - positive = forward, negative = backward*/
	public void rotateAboutLeftSensor(int speed, int direction){
		rightMotor.setSpeed(speed);
		leftMotor.setSpeed((int)(speed*SENSOR_DIST*rightWheelRadius/(leftWheelRadius*(SENSOR_DIST+width))));
		if(direction>0){
			leftMotor.forward();
			rightMotor.forward();
		}
		else {
			leftMotor.backward();
			rightMotor.backward();
		}
		
	}
	
	/**rotates the robot about it's right wheel(actually sensor)
	 * @param speed - speed in tacho/s for left wheel
	 * @param direction - positive = forward, negative = backward*/
	public void rotateAboutRightSensor(int speed, int direction){
		leftMotor.setSpeed(speed);
		rightMotor.setSpeed((int)(speed*SENSOR_DIST*leftWheelRadius/(rightWheelRadius*(SENSOR_DIST+width))));
		if(direction>0){
			leftMotor.forward();
			rightMotor.forward();
		}
		else {
			leftMotor.backward();
			rightMotor.backward();
		}
	}
	
	/**tweaks speeds proportionally to initial speed ie before any tweaking
	 * @param tweak - double from 0-1 representing % of change */
	public void tweakLeftSpeed(double tweak){
		leftMotor.setSpeed((int)((1+tweak)*forwardSpeed*180.0/(leftWheelRadius * Math.PI)));
	}
	/**tweaks speeds proportionally to initial speed ie before any tweaking
	 * @param tweak - double from 0-1 representing % of change */
	public void tweakRightSpeed(double tweak){
		rightMotor.setSpeed((int)((1+tweak)*forwardSpeed*180.0/(rightWheelRadius * Math.PI)));
	}
	
	/**used for calibration of mechanical constants
	 * @param distance - dist to travel
	 * @param speed - speed in tacho/s
	 * @param immediateReturn - non blocking if true*/
	public void roll(double distance, int speed, boolean immediateReturn){
		double avgRadius=(leftWheelRadius+rightWheelRadius)/2.0;
		leftMotor.setSpeed((int) (avgRadius*speed/leftWheelRadius));
		rightMotor.setSpeed((int) (avgRadius*speed/rightWheelRadius));
		leftMotor.rotate((int) (distance/(2*Math.PI*leftWheelRadius)*360), true);
		rightMotor.rotate((int) (distance/(2*Math.PI*rightWheelRadius)*360), immediateReturn);
	}
	/**used for calibration of mechanical constants
	 * @param angle - angle to rotate (robot heading)
	 * @param speed - speed in tacho/s
	 * @param immediateReturn - non blocking if true*/
	public void turn(int angle, int speed, boolean immediateReturn){
		double avgRadius=(leftWheelRadius+rightWheelRadius)/2.0;
		leftMotor.setSpeed((int) (avgRadius*speed/leftWheelRadius));
		rightMotor.setSpeed((int) (avgRadius*speed/rightWheelRadius));
		int lAng=(int)(angle*width/(2*leftWheelRadius));
		int rAng=(int)(angle*width/(2*rightWheelRadius));
		if(angle>0) rAng=-rAng;
		else lAng=-lAng;
		leftMotor.rotate(lAng, true);
		rightMotor.rotate(rAng, immediateReturn);
	}
	/**pauses the current motion not really in use
	 * @param delay - amount of ms to pause for*/
	public void pauseMotion(int delay) {
		double fSpeed=forwardSpeed, rSpeed=rotationSpeed;		
		setSpeeds(0,0);
		Button.waitForPress(delay);
		setSpeeds(fSpeed, rSpeed);
		}
	
}
